#ifndef MUTE_GRID_H
#define MUTE_GRID_H

#include "slam/io/proto_stream.h"
#include "slam/io/proto_stream_deserializer.h"
#include "slam/io/serialization_format_migration.h"
#include "slam/io/submap_painter.h"
#include "slam/io/internal/pbstream_info.h"

#include <map>
#include <sstream>
#include <string>
#include <vector>

#include "rclcpp/rclcpp.hpp"
#include "Eigen/Core"
#include "Eigen/Geometry"
#include "slam/mapping/2d/submap_2d.h"
#include "slam/mapping/3d/submap_3d.h"

#include <nav2_costmap_2d/costmap_2d.hpp>
#include <yaml-cpp/yaml.h>

using namespace slam;
using namespace slam::io;
using namespace slam::mapping::proto;

using namespace nav2_costmap_2d; 
using namespace std;

struct loc_w
{
  double x;
  double y;
};

class MapModifier : public rclcpp::Node
{
public:
    explicit MapModifier(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
    ~MapModifier();
    
    std::map<int, Eigen::Vector3d> CalculatePoints(vector<vector<loc_w>> polygonList);
    
    bool init();
    
    template <typename FloatType>  
    int WritePbStream(string& read_file, string& write_file, 
                     std::map<int, Eigen::Vector3d>, std::map<int, Eigen::Vector3d>);
  
private:
    Costmap2D costMap;
};

#endif // MUTE_GRID_H